/*
 * This file is part of Cleanflight and Betaflight.
 *
 * Cleanflight and Betaflight are free software. You can redistribute
 * this software and/or modify this software under the terms of the
 * GNU General Public License as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version.
 *
 * Cleanflight and Betaflight are distributed in the hope that they
 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this software.
 *
 * If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>

#include "platform.h"

#include "build/build_config.h"
#include "build/debug.h"
#include "build/debug_pin.h"

#include "common/axis.h"
#include "common/color.h"
#include "common/maths.h"
#include "common/printf_serial.h"

#include "config/config.h"
#include "config/config_eeprom.h"
#include "config/feature.h"

#include "drivers/accgyro/accgyro.h"
#include "drivers/adc.h"
#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_quadspi.h"
#include "drivers/bus_spi.h"
#include "drivers/buttons.h"
#include "drivers/camera_control_impl.h"
#include "drivers/compass/compass.h"
#include "drivers/dma.h"
#include "drivers/dshot.h"
#include "drivers/exti.h"
#include "drivers/flash/flash.h"
#include "drivers/inverter.h"
#include "drivers/io.h"
#include "drivers/light_led.h"
#include "drivers/mco.h"
#include "drivers/nvic.h"
#include "drivers/persistent.h"
#include "drivers/pin_pull_up_down.h"
#include "drivers/sensor.h"
#include "drivers/serial.h"
#include "drivers/serial_softserial.h"
#include "drivers/serial_uart.h"
#include "drivers/sdcard.h"
#include "drivers/sdio.h"
#include "drivers/sound_beeper.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/timer.h"
#include "drivers/transponder_ir.h"
#include "drivers/usb_io.h"


#include "fc/board_info.h"
#include "fc/dispatch.h"
#include "fc/gps_lap_timer.h"
#include "fc/init.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "fc/stats.h"
#include "fc/tasks.h"

#include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/gps_rescue.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "flight/position.h"
#include "flight/pos_hold.h"
#include "flight/servos.h"

#include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h"
#include "io/dashboard.h"
#include "io/flashfs.h"
#include "io/gimbal.h"
#include "io/gimbal_control.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/pidaudio.h"
#include "io/piniobox.h"
#include "io/rcdevice_cam.h"
#include "io/serial.h"
#include "io/transponder_ir.h"


#include "msp/msp.h"
#include "msp/msp_serial.h"


#include "pg/adc.h"
#include "pg/beeper.h"
#include "pg/beeper_dev.h"
#include "pg/bus_i2c.h"
#include "pg/bus_spi.h"
#include "pg/bus_quadspi.h"
#include "pg/flash.h"
#include "pg/mco.h"
#include "pg/motor.h"
#include "pg/pinio.h"
#include "pg/piniobox.h"
#include "pg/pin_pull_up_down.h"
#include "pg/pg.h"
#include "pg/rx.h"
#include "pg/sdcard.h"
#include "pg/vcd.h"

#include "rx/rx.h"

#include "scheduler/scheduler.h"

#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/esc_sensor.h"
#include "sensors/gyro.h"
#include "sensors/gyro_init.h"
#include "sensors/initialisation.h"

#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif

#ifdef TARGET_PREINIT
void targetPreInit(void);
#endif

uint8_t systemState = SYSTEM_STATE_INITIALISING;

#ifdef BUS_SWITCH_PIN
void busSwitchInit(void)
{
    IO_t busSwitchResetPin = IO_NONE;

    busSwitchResetPin = IOGetByTag(IO_TAG(BUS_SWITCH_PIN));
    IOInit(busSwitchResetPin, OWNER_SYSTEM, 0);
    IOConfigGPIO(busSwitchResetPin, IOCFG_OUT_PP);

    // ENABLE
    IOLo(busSwitchResetPin);
}
#endif

static void configureSPIBusses(void)
{
#ifdef USE_SPI
    spiPinConfigure(spiPinConfig(0));
#endif

    sensorsPreInit();

#ifdef USE_SPI
    spiPreinit();

#ifdef USE_SPI_DEVICE_0
    spiInit(SPIDEV_0);
#endif
#ifdef USE_SPI_DEVICE_1
    spiInit(SPIDEV_1);
#endif
#ifdef USE_SPI_DEVICE_2
    spiInit(SPIDEV_2);
#endif
#ifdef USE_SPI_DEVICE_3
    spiInit(SPIDEV_3);
#endif
#ifdef USE_SPI_DEVICE_4
    spiInit(SPIDEV_4);
#endif
#ifdef USE_SPI_DEVICE_5
    spiInit(SPIDEV_5);
#endif
#ifdef USE_SPI_DEVICE_6
    spiInit(SPIDEV_6);
#endif
#endif // USE_SPI
}

static void configureQuadSPIBusses(void)
{
#ifdef USE_QUADSPI
    quadSpiPinConfigure(quadSpiConfig(0));

#ifdef USE_QUADSPI_DEVICE_1
    quadSpiInit(QUADSPIDEV_1);
#endif
#endif // USE_QUADSPI
}

static void configureOctoSPIBusses(void)
{
#ifdef USE_OCTOSPI
#ifdef USE_OCTOSPI_DEVICE_1
    octoSpiInit(OCTOSPIDEV_1);
#endif
#endif
}

#ifdef USE_SDCARD
static void sdCardAndFSInit(void)
{
    sdcard_init(sdcardConfig());
    afatfs_init();
}
#endif

void init(void)
{
#if SERIAL_PORT_COUNT > 0
    printfSerialInit();
#endif

    systemInit();

    // Initialize task data as soon as possible. Has to be done before tasksInit(),
    // and any init code that may try to modify task behaviour before tasksInit().
    tasksInitData();

    // initialize IO (needed for all IO operations)
    IOInitGlobal();

    enum {
        FLASH_INIT_ATTEMPTED                = (1 << 0),
        SD_INIT_ATTEMPTED                   = (1 << 1),
        SPI_BUSSES_INIT_ATTEMPTED           = (1 << 2),
        QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED = (1 << 3),
    };
    uint8_t initFlags = 0;

    initEEPROM();

    ensureEEPROMStructureIsValid();

    bool readSuccess = readEEPROM();

    initBoardInformation();

    if (!readSuccess || !isEEPROMVersionValid() || strncasecmp(systemConfig()->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER))) {
        resetEEPROM();
    }

    systemState |= SYSTEM_STATE_CONFIG_LOADED;

    debugMode = systemConfig()->debug_mode;


    ledInit(statusLedConfig());
    LED2_ON;

    EXTIInit();


    // Note that spektrumBind checks if a call is immediately after
    // hard reset (including power cycle), so it should be called before
    // systemClockSetHSEValue and OverclockRebootIfNecessary, as these
    // may cause soft reset which will prevent spektrumBind not to execute
    // the bind procedure.


    systemClockSetHSEValue(systemConfig()->hseMhz * 1000000U);

    OverclockRebootIfNecessary(systemConfig()->cpu_overclock);

    // Configure MCO output after config is stable
    // F4 and F7 support MCO on PA8 and MCO2 on PC9, but only MCO2 is supported for now
    mcoConfigure(MCODEV_2, mcoConfig(MCODEV_2));

    timerInit();  // timer must be initialized before any channel is allocated

    uartPinConfigure(serialPinConfig());

    serialInit(featureIsEnabled(FEATURE_SOFTSERIAL));

    mixerInit(mixerConfig()->mixerMode);

    /* Motors needs to be initialized soon as posible because hardware initialization
     * may send spurious pulses to esc's causing their early initialization. Also ppm
     * receiver may share timer with motors so motors MUST be initialized here. */
    motorDevInit(getMotorCount());
    // TODO: add check here that motors actually initialised correctly
    systemState |= SYSTEM_STATE_MOTORS_READY;



    beeperInit(beeperDevConfig());

    initInverters(serialPinConfig());

    // Depending on compilation options SPI/QSPI/OSPI initialisation may already be done.
    if (!(initFlags & SPI_BUSSES_INIT_ATTEMPTED)) {
        configureSPIBusses();
        initFlags |= SPI_BUSSES_INIT_ATTEMPTED;
    }

    if (!(initFlags & QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED)) {
        configureQuadSPIBusses();
        configureOctoSPIBusses();
        initFlags |= QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED;
    }

    i2cHardwareConfigure(i2cConfig(0));
    i2cInit(I2CDEV_1);
    i2cInit(I2CDEV_2);
    i2cInit(I2CDEV_3);
 

    adcInit(adcConfig());

    initBoardAlignment(boardAlignment());

    if (!sensorsAutodetect()) {
        // if gyro was not detected due to whatever reason, notify and don't arm.
        if (isSystemConfigured()) {
            indicateFailure(FAILURE_MISSING_ACC, 2);
        }
        setArmingDisabled(ARMING_DISABLED_NO_GYRO);
    }

    systemState |= SYSTEM_STATE_SENSORS_READY;

    // Set the targetLooptime based on the detected gyro sampleRateHz and pid_process_denom
    gyroSetTargetLooptime(pidConfig()->pid_process_denom);

    // Validate and correct the gyro config or PID loop time if needed
    validateAndFixGyroConfig();

    // Now reset the targetLooptime as it's possible for the validation to change the pid_process_denom
    gyroSetTargetLooptime(pidConfig()->pid_process_denom);

    // Initialize the motor frequency filter now that we have a target looptime
    initDshotTelemetry(gyro.targetLooptime);

    // Finally initialize the gyro filtering
    gyroInitFilters();

    pidInit(currentPidProfile);

    mixerInitProfile();
 

    pinioInit(pinioConfig());

    pinPullupPulldownInit();

    pinioBoxInit(pinioBoxConfig());

    LED1_ON;
    LED0_OFF;
    LED2_OFF;

    for (int i = 0; i < 10; i++) {
        LED1_TOGGLE;
        LED0_TOGGLE;
        delay(25);
        if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_SYSTEM_INIT))) {
            BEEP_ON;
        }
        delay(25);
        BEEP_OFF;
    }
    LED0_OFF;
    LED1_OFF;

    imuInit();

    failsafeInit();

    rxInit();

    if (featureIsEnabled(FEATURE_GPS)) {
        gpsInit();
    }

    if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
        escSensorInit();
    }

    usbCableDetectInit();

 
    if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
        accStartCalibration();
    }
    gyroStartCalibration(false);
    baroStartCalibration();

    positionInit();
    autopilotInit();

    batteryInit(); // always needs doing, regardless of features.

    rcdeviceInit();

    statsInit();

    // Initialize MSP
    mspInit();
    mspSerialInit();


    setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);

    spiInitBusDMA();

    motorPostInit();
    motorEnable();

    altHoldInit();

    posHoldInit();

    if (featureIsEnabled(FEATURE_GPS)) {
        gpsRescueInit();
    }

    debugInit();

    unusedPinsInit();

    tasksInit();

    systemState |= SYSTEM_STATE_READY;
}
